/* pq_tcn.c */

#include "pq_tcn.h"
#include "tcn_param.h"
#include "fsl_powerquad.h"

//#include "core0_app.h"
//#include "app.h"

/* Private RAM */
#define PQ_PRIVATE_RAM_BASE         0xE0000000  /* offset 0KB. */
#define PQ_PRIVATE_RAM_NN_IN_DATA_BASE     0xE0000000  /* offset 0KB. for FFT. */
//#define PQ_PRIVATE_RAM_FFT_BASE     0xE0000000  /* offset 0KB. for FFT. */
//#define PQ_PRIVATE_RAM_WINDOW_BASE  0xE0001000  /* offset 4KB. for Window. */
//#define PQ_PRIVATE_RAM_DCT_BASE     0xE0002000  /* offset 8KB. for DCT. */

/* enable outputting the debug log when computing FFT. */
#define CFG_ENABLE_DEBUG_LOG_FFT    0

/*******************************************************************************
 * Variables
 ******************************************************************************/
pq_config_t pq_config;

/*******************************************************************************
 * Prototypes
 ******************************************************************************/
static float32_t pq_ln_f32_wrapper(float32_t x);
static float32_t pq_exp_f32_wrapper(float32_t x);
static float32_t pq_sqrt_f32_wrapper(float32_t x);
static float32_t pq_inv_f32_wrapper(float32_t x);
static float32_t pq_sigmoid(float32_t x);
static void      pq_softmax(float32_t *dat, uint32_t dat_len);

/*******************************************************************************
 * Code
 ******************************************************************************/

/* fft.
* - rfft
输入:
- 直接从硬件捕获到的32位数的数组, 传入数据长度为KWS_AUDIO_BUF_SIZE, 建议使用kws_audio_buf[][].
- 考虑到其中计算RFFT的时候是原地使用内存, 因此要预先分配KWS_AUDIO_BUF_SIZE*2长度的内存空间.
输出:
- 输出内容直接存放在kws_fft_buf[]中.
*/




//#define PQ_AUDIO_BUF_SIZE          KWS_FFT_POINT_256 /* used for in-place computing for FFT. */
//#define PQ_AUDIO_BUF_COUNT         2


/* 对原始音频数据进行预处理, 执行RFFT之后, 将有效频点信息存入pq_rft_out_buf_data数组中, 给后续处理使用. */

#define PQ_RFFT_POINT_256        256
#define PQ_RFFT_OUT_BUF_SIZE     256 /* 这个256同计算FFT的256个点不一样 */
#define PQ_RFFT_OUT_BLK_SIZE     63

float32_t pq_rfft_out_buf[PQ_RFFT_OUT_BUF_SIZE + PQ_RFFT_OUT_BLK_SIZE]; /* for FFT magnitude data (will be input to NN) (allocate 256 + 63). */
uint32_t  pq_rfft_out_buf_idx = 0u;
int32_t   pq_rfft_out_raw[PQ_RFFT_POINT_256*2];

void pq_tcn_preprocess(uint16_t * audio_data)
{
    /* 配置为FFT输入16数模式 */
    pq_config.inputAFormat      = kPQ_16Bit; /* 以16位数存放音频数据流 */
    pq_config.outputFormat      = kPQ_32Bit; /* 以32位数存放计算结果 */
    pq_config.machineFormat     = kPQ_32Bit;
    pq_config.inputAPrescale    = 11;
    PQ_SetConfig(POWERQUAD,&pq_config);

    /* 执行RFFT变换 */
    PQ_TransformRFFT(POWERQUAD, PQ_RFFT_POINT_256, (void *)audio_data, (void *)pq_rfft_out_raw);
    PQ_WaitDone(POWERQUAD);

    //int32_t * int32_ptr = (int32_t *)pq_rfft_out_tmp;

    float scale_factor = 1.0;
    float real, imm;
    for (uint32_t i = 2; i < 65; i++) /* 63个数, 也是没办法的事情, 0和1是不要了, 65以上是对称的, 有效的交流分量也就63个数 */
    {
        /* 将整型数转成浮点数 */
        real = pq_rfft_out_raw[i * 2    ] * scale_factor;
        imm  = pq_rfft_out_raw[i * 2 + 1] * scale_factor;
        /* 求模值 */
        pq_rfft_out_buf[pq_rfft_out_buf_idx++] = pq_sqrt_f32_wrapper( (real*real) + (imm*imm) );
    }

    pq_rfft_out_buf_idx %= PQ_RFFT_OUT_BUF_SIZE; /* 以256进行循环 */

    /* 还原默认配置 */
    pq_config.inputAFormat      = kPQ_Float; /* 以16位数存放音频数据流 */
    pq_config.outputFormat      = kPQ_Float; /* 以32位数存放计算结果 */
    pq_config.machineFormat     = kPQ_Float;
    pq_config.inputAPrescale    = 0;
    PQ_SetConfig(POWERQUAD,&pq_config);
}

/* 激活函数的类型 */
typedef enum
{
    pq_activation_func_sigmoid,
    pq_activation_func_log,
    pq_activation_func_softmax,
    pq_activation_func_sigmoid_softmax, /* 两个处理方式一起招呼上. */
    pq_activation_func_log_softmax
} pq_activation_func_t;

/* 定义全连接层的实例类型 */
/*
* 多层连接层自下而上
* taps就是水管的意思, 只是这个是引流水管, 不是放流水管
*/
typedef struct
{
    pq_activation_func_t activation_func; /* 激活函数 */

    uint32_t    dilation ; /* 膨胀系数. 本层操作下层数据块, 不同数据块的间隔, 或者说步长, 差值. 注意, 本层操作下层的数据块可能不是连续分布的, 中间可以跳着走.
                            这个膨胀系数的概念需要一点形象的理解, 基本计算单元的样子是一个老大带着n个连续排列在一起的小弟, n=in_blk_taps,
                            当膨胀系数大于1的时候, 还是一个老大带着这几个小弟, 但是小弟不再是紧密挨着, 而是等间距散开分布,
                            从外部开起来就好像是在基本计算单元的基础之上膨胀了. 膨胀的只是小弟之间的间距, 老大只有一个, 就算是膨胀了, 还是一个老大, 因为老大自己跟自己没有间距
                            "前后一臂，左右两臂，成体操队形, 散开!" */

    /* 关于输入 */
    uint32_t    in_blk_taps;/* taps */ /* 本层计算节点总共有包含多少个下级的数据块 */
    uint32_t  * in_blk_idx_buf; /* 本层操作下层的每个数据块中, 每个数据块内部的索引,
                                   索引的数据缓冲是原始的下级数组内存 */


    uint32_t    in_blk_size;   /* 前级级数据块的大小. 下级layer中每个node有多少个数. */
    uint32_t    in_buf_size;   /* 前级数据缓冲区大小 */
    float32_t * in_buf_data;   /* 下级layer数据队列的原始存放地址 */
    bool        in_buf_enable_ring;   /* 是否对input的读取过程启用ring buffer模式 */

    /* 关于输出 */
    uint32_t    out_blk_size; /* 本层每个计算单元的output块大小. */
    uint32_t    out_buf_size; /* 每个layer都有一个缓冲区, 连续地保存本层中所有的计算结果, 可能包含多个out_size. */
    float32_t * out_buf_data; /* 本层计算结果的输出缓冲区 */
    uint32_t    out_buf_idx;  /* 本层中输出缓冲区的游标 */
    bool        out_buf_enable_ring;  /* 是否对output的写入过程启用ring buffer模式 */

    /* 权重矩阵 */
    float32_t * weight; /* 权重 */
    float32_t * bias;   /* 偏执 */

    uint32_t    pq_scale_length; /* powerquad计算向量点乘时配置LENGTH寄存器的值 */

} tcn_layer_instance_t;



/* 这一层是取代mfcc中求倒谱和dct的操作
* 输入: 这一层的输入是来自于对原始声音信号进行FFT提取特征之后的结果, 对于256个数的一帧, 将得到63个有效数的频点.
*       本层取代mfcc的处理, 将这63个数变成40个倒谱系数. (adam用了40个倒谱系数, 我之前用了13个). 本层是直接是
*       直接处理, 即1个大小为63个数的数据块作为输入, 转变成40个数的数据块作为输出. 本层只是做数据变换, 没有时间
*       上的多合一, 因此, 未被称作TCN的全连接层, 而仅仅是作为一个预处理层.
*/
#define TCN_LAYER_PRE_IN_BLK_TAPS   1   /* 本层计算节点的输入只有一个引流水管 */
#define TCN_LAYER_PRE_OUT_BUF_SIZE  256 /* layer_pre中保存输出数据缓冲区的大小 */
#define TCN_LAYER_PRE_OUT_BLK_SIZE  40  /* 每个计算单元计算输出数据块的大小 */
static uint32_t  tcn_layer_pre_in_idx_buf[TCN_LAYER_PRE_IN_BLK_TAPS]; /* 每个引流源数据块内部的下标 */
static float32_t tcn_layer_pre_out_buf[TCN_LAYER_PRE_OUT_BUF_SIZE + TCN_LAYER_PRE_OUT_BLK_SIZE]; /* layer_pre中保存输出数据的缓冲区 */
static void tcn_init_layer_pre_instance(tcn_layer_instance_t *instance, float32_t *weight, float32_t *bias)
{
    /* 激活函数 */
    instance->activation_func = pq_activation_func_log;

    /* 膨胀系数, 也就是TCN模型的范围尺度 */
    instance->dilation = 1; /* 每个数据块都需要处理, 不能跳跃 */

    /* 关于输入 */
    instance->in_blk_taps    = 1; /* 最底层进入网络的门槛, 只是将一帧数据转化成一个节点, 不进行N合1的总结抽象 */
    instance->in_blk_idx_buf = tcn_layer_pre_in_idx_buf; /* 为小弟blk中的数据游标分配空间. */

    instance->in_blk_size    = PQ_RFFT_OUT_BLK_SIZE; /* 每个输入的数据块包含63个数, 小弟 */
    instance->in_buf_size    = PQ_RFFT_OUT_BUF_SIZE; /* 直接从FFT计算的输出缓冲区中拿数 */
    instance->in_buf_data    = pq_rfft_out_buf;     /* 指向fft计算的结果数据. */
    instance->in_buf_enable_ring = true;
    for (uint32_t i = 0; i < instance->in_blk_taps; i++)
    {
        instance->in_blk_idx_buf[i] = i * instance->in_blk_size * instance->dilation;
    }

    /* 关于输出 */
    instance->out_blk_size = TCN_LAYER_PRE_OUT_BLK_SIZE; /* 当前层计算完成后输出的单个数据块长度 */
    instance->out_buf_size = TCN_LAYER_PRE_OUT_BUF_SIZE;
    instance->out_buf_data = tcn_layer_pre_out_buf;
    instance->out_buf_enable_ring = true;
    instance->out_buf_idx  = (   (instance->out_buf_enable_ring)
                               ? (4 * instance->out_blk_size * instance->dilation) /* 先让一段空间 */
                               : (0) ); /* 从零开始 */

    instance->weight = weight;
    instance->bias   = bias;

    instance->pq_scale_length = POWERQUAD_MAKE_MATRIX_LEN( ((instance->in_blk_size * instance->in_blk_taps) >> 4) + 1, 16, 0 );

    /* 在初始化过程中, 清零本层的输出缓冲区 */
    memset(instance->out_buf_data, 0, instance->out_buf_size * sizeof(float32_t));
}

/* 在layer0中, 对一小段连续时间的信息进行合并, 减少数据规模, 但表示信息的时间跨度更长
 * 输入: 整合3个分帧(taps), 每个分帧的参数为40个(上一层输出一个数据块的大小), 因此, 每个神经元的输入为120个数
 * 输出: 每个神经元的输出块为31个数, 将送往下一层神经元
 */
#define TCN_LAYER_0_IN_BLK_TAPS   3   /* 本层计算节点的输入只有一个引流水管 */
#define TCN_LAYER_0_OUT_BUF_SIZE  256 /* layer_pre中保存输出数据缓冲区的大小 */
#define TCN_LAYER_0_OUT_BLK_SIZE  31  /* 每个计算单元计算输出数据块的大小 */
static uint32_t  tcn_layer_0_in_idx_buf[TCN_LAYER_0_IN_BLK_TAPS]; /* 每个引流源数据块内部的下标 */
static float32_t tcn_layer_0_out_buf[TCN_LAYER_0_OUT_BUF_SIZE + TCN_LAYER_0_OUT_BLK_SIZE]; /* 本层的输出缓冲区 */

void tcn_init_layer_0_instance(tcn_layer_instance_t *instance, float32_t *weight, float32_t *bias)
{
    /* 激活函数 */
    instance->activation_func = pq_activation_func_sigmoid;

    /* 膨胀系数, 也就是TCN模型的范围尺度 */
    instance->dilation = 1; /* 每个数据块都需要处理, 不能跳跃 */

    /* 关于输入, 数据来源都是前一层的 */
    instance->in_blk_taps    = TCN_LAYER_0_IN_BLK_TAPS; /* 在本层将3个数据块搞成一个数据块 */
    instance->in_blk_idx_buf = tcn_layer_0_in_idx_buf;  /* 为小弟blk中的数据游标分配空间. */

    instance->in_blk_size    = TCN_LAYER_PRE_OUT_BLK_SIZE; /* 前一层的输出块大小, 就是本层的输入快大小. */
    instance->in_buf_size    = TCN_LAYER_PRE_OUT_BUF_SIZE; /* 前一层的输出缓冲区, 就是本层的输入数据. */
    instance->in_buf_data    = tcn_layer_pre_out_buf; /* pre层的输出, 就是layer0层的输入. */
    instance->in_buf_enable_ring = true;
    for (uint32_t i = 0; i < instance->in_blk_taps; i++) /* 指定index们的初始位置 */
    {
        instance->in_blk_idx_buf[i] = i * instance->in_blk_size * instance->dilation;
    }

    /* 关于输出, 本层产生的输出数据 */
    instance->out_blk_size = TCN_LAYER_0_OUT_BLK_SIZE; /* 当前层计算完成后输出的单个数据块长度 */
    instance->out_buf_size = TCN_LAYER_0_OUT_BUF_SIZE;
    instance->out_buf_data = tcn_layer_0_out_buf;
    instance->out_buf_enable_ring = true;
    instance->out_buf_idx  = (   (instance->out_buf_enable_ring)
                               ? (4 * instance->out_blk_size * instance->dilation) /* 先让一段空间 */
                               : (0) ); /* 从零开始 */

    instance->weight = weight;
    instance->bias   = bias;

    /* 给powerquad执行向量点乘时使用的length寄存器设定值.
     * 这里总是一个16的倍数, 并且比实际需要计算数据要多. 不用担心, 填充数据的输入为0, 经过向量乘法后仍为0.
     */
    instance->pq_scale_length = POWERQUAD_MAKE_MATRIX_LEN( ((instance->in_blk_size * instance->in_blk_taps) >> 4) + 1, 16, 0 );

    /* 在初始化过程中, 清零本层的输出缓冲区 */
    memset(instance->out_buf_data, 0, instance->out_buf_size * sizeof(float32_t));

}

/*
 * layer1:
 * 中间层, 以layer 0的输出作为输入, 输入块大小为31, 输出块大小为31.
 * 3个taps, 膨胀系数为2,
 * 输出缓冲区大小为512
 */
#define TCN_LAYER_1_IN_BLK_TAPS   3   /* 本层计算节点的输入有3个引流水管 */
#define TCN_LAYER_1_OUT_BUF_SIZE  512 /* layer_pre中保存输出数据缓冲区的大小 */
#define TCN_LAYER_1_OUT_BLK_SIZE  31  /* 每个计算单元计算输出数据块的大小 */
static uint32_t  tcn_layer_1_in_idx_buf[TCN_LAYER_1_IN_BLK_TAPS]; /* 每个引流源数据块内部的下标 */
static float32_t tcn_layer_1_out_buf[TCN_LAYER_1_OUT_BUF_SIZE + TCN_LAYER_1_OUT_BLK_SIZE]; /* 本层的输出缓冲区 */

void tcn_init_layer_1_instance(tcn_layer_instance_t *instance, float32_t *weight, float32_t *bias)
{
    /* 激活函数 */
    instance->activation_func = pq_activation_func_sigmoid;

    /* 膨胀系数, 也就是TCN模型的范围尺度 */
    instance->dilation = 2;

    /* 关于输入 */
    instance->in_blk_taps    = TCN_LAYER_1_IN_BLK_TAPS; /* 在本层将3个数据块搞成一个数据块 */
    instance->in_blk_idx_buf = tcn_layer_1_in_idx_buf; /* 为小弟blk中的数据游标分配空间. */

    instance->in_blk_size    = TCN_LAYER_0_OUT_BLK_SIZE; /* 前一层的输出块大小, 就是本层的输入快大小. */
    instance->in_buf_size    = TCN_LAYER_0_OUT_BUF_SIZE; /* 前一层的输出缓冲区, 就是本层的输入数据. */
    instance->in_buf_data    = tcn_layer_0_out_buf; /* 前一层的输出缓冲区, 就是本层的输入缓冲区. */
    instance->in_buf_enable_ring = true;
    for (uint32_t i = 0; i < instance->in_blk_taps; i++) /* 指定index们的初始位置 */
    {
        instance->in_blk_idx_buf[i] = i * instance->in_blk_size * instance->dilation;
    }

    /* 关于输出 */
    instance->out_blk_size = TCN_LAYER_1_OUT_BLK_SIZE; /* 当前层计算完成后输出的单个数据块长度 */
    instance->out_buf_size = TCN_LAYER_1_OUT_BUF_SIZE;
    instance->out_buf_data = tcn_layer_1_out_buf; /* 本层的输出U缓冲区 */
    instance->out_buf_enable_ring = true;
    instance->out_buf_idx  = (   (instance->out_buf_enable_ring)
                               ? (4 * instance->out_blk_size * instance->dilation) /* 先让一段空间 */
                               : (0) ); /* 从零开始 */

    instance->weight = weight;
    instance->bias   = bias;

    /* 给powerquad执行向量点乘时使用的length寄存器设定值.
     * 这里总是一个16的倍数, 并且比实际需要计算数据要多. 不用担心, 填充数据的输入为0, 经过向量乘法后仍为0.
     */
    instance->pq_scale_length = POWERQUAD_MAKE_MATRIX_LEN( ((instance->in_blk_size * instance->in_blk_taps) >> 4) + 1, 16, 0 );

    /* 在初始化过程中, 清零本层的输出缓冲区 */
    memset(instance->out_buf_data, 0, instance->out_buf_size * sizeof(float32_t));
}

/*
 * layer2:
 * 中间层, 以layer 1的输出作为输入, 输入块大小为31, 输出块大小为21.
 * 3个taps, 膨胀系数为4,
 * 输出缓冲区大小为512
 *
 */
#define TCN_LAYER_2_IN_BLK_TAPS   3   /* 本层计算节点的输入有3个引流水管 */
#define TCN_LAYER_2_OUT_BUF_SIZE  512 /* 本层输出数据缓冲区的大小 */
#define TCN_LAYER_2_OUT_BLK_SIZE  21  /* 本层每个计算单元计算输出数据块的大小 */
static uint32_t  tcn_layer_2_in_idx_buf[TCN_LAYER_2_IN_BLK_TAPS]; /* 每个引流源数据块内部的下标 */
static float32_t tcn_layer_2_out_buf[TCN_LAYER_2_OUT_BUF_SIZE + TCN_LAYER_2_OUT_BLK_SIZE]; /* 本层的输出缓冲区 */

void tcn_init_layer_2_instance(tcn_layer_instance_t *instance, float32_t *weight, float32_t *bias)
{
    /* 激活函数 */
    instance->activation_func = pq_activation_func_sigmoid;

    /* 膨胀系数, 也就是TCN模型的范围尺度 */
    instance->dilation = 4;

    /* 关于输入 */
    instance->in_blk_taps    = TCN_LAYER_2_IN_BLK_TAPS; /* 在本层将3个数据块搞成一个数据块 */
    instance->in_blk_idx_buf = tcn_layer_2_in_idx_buf;  /* 为小弟blk中的数据游标分配空间. */

    instance->in_blk_size    = TCN_LAYER_1_OUT_BLK_SIZE; /* 前一层的输出块大小, 就是本层的输入快大小. */
    instance->in_buf_size    = TCN_LAYER_1_OUT_BUF_SIZE; /* 前一层的输出缓冲区, 就是本层的输入数据. */
    instance->in_buf_data    = tcn_layer_1_out_buf;      /* 前一层的输出缓冲区, 就是本层的输入缓冲区. */
    instance->in_buf_enable_ring = true;
    for (uint32_t i = 0; i < instance->in_blk_taps; i++) /* 指定index们的初始位置 */
    {
        instance->in_blk_idx_buf[i] = i * instance->in_blk_size * instance->dilation;
    }

    /* 关于输出 */
    instance->out_blk_size = TCN_LAYER_2_OUT_BLK_SIZE; /* 当前层计算完成后输出的单个数据块长度 */
    instance->out_buf_size = TCN_LAYER_2_OUT_BUF_SIZE;
    instance->out_buf_data = tcn_layer_2_out_buf; /* 本层的输出U缓冲区 */
    instance->out_buf_enable_ring = true;
    instance->out_buf_idx  = (   (instance->out_buf_enable_ring)
                               ? (4 * instance->out_blk_size * instance->dilation) /* 先让一段空间 */
                               : (0) ); /* 从零开始 */

    instance->weight = weight;
    instance->bias   = bias;

    /* 给powerquad执行向量点乘时使用的length寄存器设定值.
     * 这里总是一个16的倍数, 并且比实际需要计算数据要多. 不用担心, 填充数据的输入为0, 经过向量乘法后仍为0.
     */
    instance->pq_scale_length = POWERQUAD_MAKE_MATRIX_LEN( ((instance->in_blk_size * instance->in_blk_taps) >> 4) + 1, 16, 0 );

    /* 在初始化过程中, 清零本层的输出缓冲区 */
    memset(instance->out_buf_data, 0, instance->out_buf_size * sizeof(float32_t));
}

/*
 * layer3:
 * 中间层, 以layer 2的输出作为输入, 输入块大小为21, 输出块大小为21.
 * 3个taps, 膨胀系数为8,
 * 输出缓冲区大小为1024
 *
 */
#define TCN_LAYER_3_IN_BLK_TAPS   3   /* 本层计算节点的输入有3个引流水管 */
#define TCN_LAYER_3_OUT_BUF_SIZE  1024 /* layer_pre中保存输出数据缓冲区的大小 */
#define TCN_LAYER_3_OUT_BLK_SIZE  21  /* 每个计算单元计算输出数据块的大小 */
static uint32_t  tcn_layer_3_in_idx_buf[TCN_LAYER_3_IN_BLK_TAPS]; /* 每个引流源数据块内部的下标 */
static float32_t tcn_layer_3_out_buf[TCN_LAYER_3_OUT_BUF_SIZE + TCN_LAYER_3_OUT_BLK_SIZE]; /* 本层的输出缓冲区 */

void tcn_init_layer_3_instance(tcn_layer_instance_t *instance, float32_t *weight, float32_t *bias)
{
    /* 激活函数 */
    instance->activation_func = pq_activation_func_sigmoid;

    /* 膨胀系数, 也就是TCN模型的范围尺度 */
    instance->dilation = 8;

    /* 关于输入 */
    instance->in_blk_taps    = TCN_LAYER_3_IN_BLK_TAPS; /* 在本层将3个数据块搞成一个数据块 */
    instance->in_blk_idx_buf = tcn_layer_3_in_idx_buf; /* 为小弟blk中的数据游标分配空间. */

    instance->in_blk_size    = TCN_LAYER_2_OUT_BLK_SIZE; /* 前一层的输出块大小, 就是本层的输入快大小. */
    instance->in_buf_size    = TCN_LAYER_2_OUT_BUF_SIZE; /* 前一层的输出缓冲区, 就是本层的输入数据. */
    instance->in_buf_data    = tcn_layer_2_out_buf; /* 前一层的输出缓冲区, 就是本层的输入缓冲区. */
    instance->in_buf_enable_ring = true;
    for (uint32_t i = 0; i < instance->in_blk_taps; i++) /* 指定index们的初始位置 */
    {
        instance->in_blk_idx_buf[i] = i * instance->in_blk_size * instance->dilation;
    }

    /* 关于输出 */
    instance->out_blk_size = TCN_LAYER_3_OUT_BLK_SIZE; /* 当前层计算完成后输出的单个数据块长度 */
    instance->out_buf_size = TCN_LAYER_3_OUT_BUF_SIZE;
    instance->out_buf_data = tcn_layer_3_out_buf; /* 本层的输出U缓冲区 */
    instance->out_buf_enable_ring = true;
    instance->out_buf_idx  = (   (instance->out_buf_enable_ring)
                               ? (4 * instance->out_blk_size * instance->dilation) /* 先让一段空间 */
                               : (0) ); /* 从零开始 */

    instance->weight = weight;
    instance->bias   = bias;

    /* 给powerquad执行向量点乘时使用的length寄存器设定值.
     * 这里总是一个16的倍数, 并且比实际需要计算数据要多. 不用担心, 填充数据的输入为0, 经过向量乘法后仍为0.
     */
    instance->pq_scale_length = POWERQUAD_MAKE_MATRIX_LEN( ((instance->in_blk_size * instance->in_blk_taps) >> 4) + 1, 16, 0 );

    /* 在初始化过程中, 清零本层的输出缓冲区 */
    memset(instance->out_buf_data, 0, instance->out_buf_size * sizeof(float32_t));
}


/*
 * layer4:
 * 中间层, 以layer 3的输出作为输入, 输入块大小为21, 输出块大小为12.
 * 3个taps, 膨胀系数为16,
 * 输出缓冲区大小为64
 * 注意, layer4是思维过程的最后一层, 可以直接为判决提供依据.
 * 这里的缓冲区是为了滤波器服务的, 只有当连续的几次判决稳定为某一个结果, 才能最终确认是这样的.
 * 特别注意, Adam这里不用周期循环缓冲区了, 看来是算定了1s中的语音片段, 更长的语音片段不识别.
 * 最后一层用的激活函数也不再是sigmoid, 而是softmax, 为最后决策做准备
 *
 */
#define TCN_LAYER_4_IN_BLK_TAPS   3   /* 本层计算节点的输入有3个引流水管 */
#define TCN_LAYER_4_OUT_BUF_SIZE  64  /* 本层中保存输出数据缓冲区的大小 */
#define TCN_LAYER_4_OUT_BLK_SIZE  12  /* 本层每个计算单元计算输出数据块的大小 */
static uint32_t  tcn_layer_4_in_idx_buf[TCN_LAYER_4_IN_BLK_TAPS]; /* 每个引流源数据块内部的下标 */
static float32_t tcn_layer_4_out_buf[TCN_LAYER_4_OUT_BUF_SIZE + TCN_LAYER_4_OUT_BLK_SIZE]; /* 本层的输出缓冲区 */

void tcn_init_layer_4_instance(tcn_layer_instance_t *instance, float32_t *weight, float32_t *bias)
{
    /* 激活函数 */
    instance->activation_func = pq_activation_func_softmax; /* 最后一层使用softmax函数作为激活函数 */

    /* 膨胀系数, 也就是TCN模型的范围尺度 */
    instance->dilation = 16;

    /* 关于输入 */
    instance->in_blk_taps    = TCN_LAYER_4_IN_BLK_TAPS; /* 在本层将3个数据块搞成一个数据块 */
    instance->in_blk_idx_buf = tcn_layer_4_in_idx_buf; /* 为小弟blk中的数据游标分配空间. */

    instance->in_blk_size    = TCN_LAYER_3_OUT_BLK_SIZE; /* 前一层的输出块大小, 就是本层的输入快大小. */
    instance->in_buf_size    = TCN_LAYER_3_OUT_BUF_SIZE; /* 前一层的输出缓冲区, 就是本层的输入数据. */
    instance->in_buf_data    = tcn_layer_3_out_buf; /* 前一层的输出缓冲区, 就是本层的输入缓冲区. */
    instance->in_buf_enable_ring = true;
    for (uint32_t i = 0; i < instance->in_blk_taps; i++) /* 指定index们的初始位置 */
    {
        instance->in_blk_idx_buf[i] = i * instance->in_blk_size * instance->dilation;
    }

    /* 关于输出 */
    instance->out_blk_size = TCN_LAYER_4_OUT_BLK_SIZE; /* 当前层计算完成后输出的单个数据块长度 */
    instance->out_buf_size = TCN_LAYER_4_OUT_BUF_SIZE;
    instance->out_buf_data = tcn_layer_4_out_buf; /* 本层的输出U缓冲区 */
    instance->out_buf_enable_ring = false; /* !!! 特别注意, 这里没有循环了. */
    instance->out_buf_idx  = (   (instance->out_buf_enable_ring)
                               ? (4 * instance->out_blk_size * instance->dilation) /* 先让一段空间 */
                               : (0) ); /* 从零开始 */

    instance->weight = weight;
    instance->bias   = bias;

    /* 给powerquad执行向量点乘时使用的length寄存器设定值.
     * 这里总是一个16的倍数, 并且比实际需要计算数据要多. 不用担心, 填充数据的输入为0, 经过向量乘法后仍为0.
     */
    instance->pq_scale_length = POWERQUAD_MAKE_MATRIX_LEN( ((instance->in_blk_size * instance->in_blk_taps) >> 4) + 1, 16, 0 );

    /* 在初始化过程中, 清零本层的输出缓冲区 */
    memset(instance->out_buf_data, 0, instance->out_buf_size * sizeof(float32_t));
}

/* 搜集本层输入要处理的数据到一个连续的物理地址上, 预分配的缓冲区 */
static float32_t tcn_layer_data_to_process[1024];

static void pq_tcn_run_layer_infer(tcn_layer_instance_t *instance)
{
    /* 这种重新组织数据顺序的处理方式将会是TCN执行网络计算的一个重要的特点. */
    for (uint32_t t = 0; t < instance->in_blk_taps; t++)
    {
        memcpy( &tcn_layer_data_to_process[instance->in_blk_size * t],
                //&instance->in_buf_data[instance->in_blk_size * t * instance->dilation],
                &instance->in_buf_data[instance->in_blk_idx_buf[t]],
                instance->in_blk_size * sizeof(float32_t)
            );

        /* 递增每个blk内部的索引, 挪到下一个指针的位置, 用于下一个计算单元的计算*/
        instance->in_blk_idx_buf[t] += instance->in_blk_size;

        /* ring buffer回转. */
        instance->in_blk_idx_buf[t] %= instance->in_buf_size;
    }

    /* 多个向量点积实现的更大规模的矩阵乘法, 整合权值 */
    PQ_MatrixScale(POWERQUAD, instance->pq_scale_length, 1.0, &tcn_layer_data_to_process, (void *)PQ_PRIVATE_RAM_NN_IN_DATA_BASE); // place data into private RAM of PQ
    PQ_WaitDone(POWERQUAD);

    uint32_t vector_length = instance->in_blk_size * instance->in_blk_taps;
    float32_t output_val = 0.0f;
    float32_t * weight_tmp = instance->weight;
    float32_t * bias_tmp = instance->bias;
    for (uint32_t i = 0; i < instance->out_blk_size; i++) /* 网络计算. 这里每次循环会产生一个新的输出 */
    {
        /* 点乘权重 */
        PQ_VectorDotProduct(POWERQUAD, vector_length, (void *)0xE0000000, weight_tmp, &output_val);
        weight_tmp += vector_length; /* 移动到下一组权重向量. 权重在概念上是一个矩阵, 但实际是以线性数组存储在内存中 */
        PQ_WaitDone(POWERQUAD);

        /* 增加偏移量 */
        output_val += (*bias_tmp);
        bias_tmp++; /* 移动到一个偏移值 */

        /* 激活函数 */
        if (   (instance->activation_func == pq_activation_func_sigmoid)
            || (instance->activation_func == pq_activation_func_sigmoid_softmax)  )
        {
            output_val = pq_sigmoid(output_val);
        }
        else if (   (instance->activation_func == pq_activation_func_log)
                 || (instance->activation_func == pq_activation_func_log_softmax)  )
        {
            output_val = pq_ln_f32_wrapper(output_val);
        }

        /* 存入输出缓冲区 */
        instance->out_buf_data[instance->out_buf_idx++] = output_val;
    }

    /* 在有必要的情况下用softmax归一化输出 */
    if (   (instance->activation_func == pq_activation_func_softmax)
        || (instance->activation_func == pq_activation_func_sigmoid_softmax)
        || (instance->activation_func == pq_activation_func_log_softmax) )
    {
        pq_softmax(&instance->out_buf_data[instance->out_buf_idx - instance->out_blk_size], instance->out_blk_size); /* 原地更新 */
    }

    /* 更新输出缓冲区的游标 */
    if (instance->out_buf_enable_ring)
    {
        instance->out_buf_idx %= instance->out_buf_size; /* 绕过去. */
    }
    else
    {
        instance->out_buf_idx -= instance->out_blk_size; /* 退回去. */
    }

}

typedef struct
{
    uint32_t    label_num;
    float32_t * label_buf;
    uint32_t    current_label;
    uint32_t    timeout_count;
} tcn_decision_layer_instance_t ;

#define TCN_DECISION_LAYER_LABLE_NUM  TCN_LAYER_4_OUT_BLK_SIZE /* 识别12个关键词 */

static float32_t tcn_decision_layer_label_buf[TCN_DECISION_LAYER_LABLE_NUM];
void tcn_init_decision_layer_instance(tcn_decision_layer_instance_t * instance)
{
    instance->label_num = TCN_DECISION_LAYER_LABLE_NUM;
    instance->label_buf = tcn_decision_layer_label_buf;

    instance->current_label = instance->label_num - 1;
    for (uint32_t i = 0; i < instance->label_num; i++)
    {
        instance->label_buf[i] = 0.0f;
    }
    instance->timeout_count = 0; /* 初始化滤波器迟滞计数器. */
}

#define DECIDER_MVG_AVG_CONTRIBUTION 0.1
#define DECIDER_THRESHOLD 0.65

static uint32_t tcn_decision_layer_eval(tcn_decision_layer_instance_t * instance, float32_t *data_buf)
{
    float32_t max_val = 0.0f;
    uint32_t max_val_pos = instance->current_label;
    float32_t tmp;

    instance->timeout_count++; /* 累计判断次数 */

    /* 执行一次遍历 */
    for (uint32_t i = 0; i < instance->label_num; i++)
    {
        tmp = data_buf[i] - 1.0f;
        tmp = pq_exp_f32_wrapper(tmp);

        /* 这里相当于是执行了一个FIR滤波器 */
        instance->label_buf[i]  = ( (1.0f - DECIDER_MVG_AVG_CONTRIBUTION) * instance->label_buf[i])
                                + ( DECIDER_MVG_AVG_CONTRIBUTION * tmp);

        if (instance->label_buf[i] > max_val)
        {
            max_val = instance->label_buf[i];
            max_val_pos = i;
        }
    }

    if (    (max_val_pos != instance->current_label) /* 已经有了一个有效的最大值 */
         && (max_val > DECIDER_THRESHOLD)  )  /* 最大值的优势比较明显 */
    {
        instance->current_label = max_val_pos; /* 菇凉, 我很看好你呦 */
        instance->timeout_count = 0;
        return true;
    }
    else
    {
        if (instance->timeout_count < 64) /* 大约一秒钟 */
        {
            return false;
        }
        else
        {
            instance->current_label = (instance->label_num == 1) ? 1 : (instance->label_num -1);
            instance->timeout_count = 0;
            return true;
        }
    }
}

void pq_init_hardware(void)
{
    PQ_Init(POWERQUAD);

    pq_config.inputAFormat      = kPQ_Float;
    pq_config.inputAPrescale    = 0;
    pq_config.inputBFormat      = kPQ_Float;
    pq_config.inputBPrescale    = 0;
    pq_config.tmpFormat         = kPQ_Float;
    pq_config.tmpPrescale       = 0;
    pq_config.outputFormat      = kPQ_Float;
    pq_config.outputPrescale    = 0;
    pq_config.machineFormat     = kPQ_Float;
    pq_config.tmpBase           = (uint32_t *)PQ_PRIVATE_RAM_BASE;
    PQ_SetConfig(POWERQUAD,&pq_config);
}

static tcn_layer_instance_t tcn_layer_pre_instance;
static tcn_layer_instance_t tcn_layer_0_instance;
static tcn_layer_instance_t tcn_layer_1_instance;
static tcn_layer_instance_t tcn_layer_2_instance;
static tcn_layer_instance_t tcn_layer_3_instance;
static tcn_layer_instance_t tcn_layer_4_instance;
static tcn_decision_layer_instance_t tcn_decision_layer_instance;

void pq_tcn_init(void)
{
    tcn_init_layer_pre_instance(&tcn_layer_pre_instance, Wpre, Bpre); /* layer pre. */

    tcn_init_layer_0_instance(&tcn_layer_0_instance, W0, B0);
    tcn_init_layer_1_instance(&tcn_layer_1_instance, W1, B1);
    tcn_init_layer_2_instance(&tcn_layer_2_instance, W2, B2);
    tcn_init_layer_3_instance(&tcn_layer_3_instance, W3, B3);
    tcn_init_layer_4_instance(&tcn_layer_4_instance, W4, B4);

    tcn_init_decision_layer_instance(&tcn_decision_layer_instance);

}

/* 0-9为有效返回值, 别的返回值无效 */
uint32_t pq_tcn_run(void)
{
    pq_tcn_run_layer_infer(&tcn_layer_pre_instance);/* layer pre. */

    pq_tcn_run_layer_infer(&tcn_layer_0_instance);
    pq_tcn_run_layer_infer(&tcn_layer_1_instance);
    pq_tcn_run_layer_infer(&tcn_layer_2_instance);
    pq_tcn_run_layer_infer(&tcn_layer_3_instance);
    pq_tcn_run_layer_infer(&tcn_layer_4_instance);

    if ( tcn_decision_layer_eval(&tcn_decision_layer_instance, tcn_layer_4_instance.out_buf_data) )
    {
#if 0
        if (tcn_decision_layer_instance.current_label > 9)
        {
            PRINTF("-");
        }
        else
        {
            PRINTF("%d", tcn_decision_layer_instance.current_label);
        }
#endif
        //return 1;
        return tcn_decision_layer_instance.current_label;/* 返回识别结果 */
    }
    else
    {
        return (uint32_t)(-1); /* 识别不显著 */
    }
}

/*
 * PowerQuad Wrapper functions.
 */
#if 0
/* 余弦函数,x为弧度值 */
static float32_t pq_cos_f32_wrapper(float32_t x)
{
    float32_t output;
    PQ_CosF32(&x, &output);
    return output;
}
#endif

/* 求对数,以自然常数e为底 */
static float32_t pq_ln_f32_wrapper(float32_t x)
{
    float32_t output;
    PQ_LnF32(&x, &output);
    return output;
}

/* 求幂,以自然常数e为底 */
static float32_t pq_exp_f32_wrapper(float32_t x)
{
    float32_t output;
    PQ_EtoxF32(&x, &output);
    return output;
}

/* 开根号 */
static float32_t pq_sqrt_f32_wrapper(float32_t x)
{
    float32_t output;
    PQ_SqrtF32(&x, &output);
    return output;
}

#if 0
/* 浮点数除法 */
static float32_t pq_div_f32_wrapper(float32_t x1, float32_t x2)
{
    float32_t output;
    PQ_DivF32(&x1, &x2, &output);
    return output;
}
#endif

static float32_t pq_inv_f32_wrapper(float32_t x)
{
    float32_t output;
    PQ_InvF32(&x, &output);
    return output;
}

#if 0
/* 定点数转浮点数 */
static void pq_q15_to_float(q15_t * pSrc, float32_t * pDst, uint32_t blockSize)
{
    while (blockSize > 0u)
    {   /* C = (float32_t) A / 32768 */
        *pDst++ = pq_div_f32_wrapper((float32_t)*pSrc++, 32768.0f);
        blockSize--;/* Decrement the loop counter */
    }
}

static void pq_q31_to_float( q31_t * pSrc, float32_t * pDst, uint32_t blockSize)
{
    while (blockSize > 0u)
    {   /* C = (float32_t) A / 2147483648 */
        *pDst++ = pq_div_f32_wrapper((float32_t) * pSrc++ , 2147483648.0f);
        blockSize--;
    }
}
#endif

static float32_t pq_sigmoid(float32_t x)
{
    return ( 1.0f / ( 1.0f + pq_exp_f32_wrapper( -x ) ) );
}

/* 输入数据通过dat指针传入, 计算完成后, 原地存放计算结果 */
static void pq_softmax(float32_t *dat, uint32_t dat_len)
{
    uint32_t i;
    float32_t sum = 0.0f;
    for (i = 0; i < dat_len; i++)
    {
        dat[i] = pq_exp_f32_wrapper(dat[i]);
        sum += dat[i];
    }
    sum = pq_inv_f32_wrapper(sum);
    for (i = 0; i <dat_len; i++)
    {
        dat[i] *= sum;
    }
}

/* EOF. */

